#include "msg_interfaces/msg/matrix_proto.hpp"
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/bool.hpp"
#include "std_msgs/msg/string.hpp"
#include <arpa/inet.h>
#include <cerrno>
#include <chrono>
#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <iostream>
#include <netinet/in.h>
#include <pthread.h>
#include <signal.h>
#include <string>
#include <sys/socket.h>
#include <sys/types.h>
#include <unistd.h>
// 添加proto
#include "chassis.pb.h"
#include "control.pb.h"
#include "header.pb.h"
#include "localization.pb.h"
#include "pnc.pb.h"
#include "pnc_common.pb.h"
#include "routing.pb.h"
#include "state_machine.pb.h"

#include "cJSON.h"

#define SERV_PORT 12345
#define LISTEN_COUNT 5
using std::placeholders::_1;
using namespace std::chrono_literals;
struct yolov5_item
{
    std::string color;
    std::string direction;
};
struct connect_message
{
    int connfd;
    struct sockaddr_in cliaddr;
};
int listenfd = 0;
int roadLimit = 0;
float speed = 0;
int lightType = 0;
int state = 0;
double pad_lat, pad_lon;
double pad_angle;
std::chrono::milliseconds ms;
// DebugINFO
bool cloud_enable = false;
std::string timestamp = "";
std::string source = "";
std::string status = ""; // #success/failure

bool remoteControlEnable = false;
std_msgs::msg::String traffic_light_data;
std::vector<yolov5_item> yolov5_light_vec;

std_msgs::msg::Bool remoteControlFlag;
connect_message cm;

std::vector<connect_message> all_connect;
class PadServer : public rclcpp::Node
{
public:
    PadServer() : Node("pad_server")
    {
        // sub topic name 格式后面去对齐
        server_sub = this->create_subscription<msg_interfaces::msg::MatrixProto>("/server", 10,
                                                                                 std::bind(&PadServer::CallBackServer, this, _1));

        pad_pub = this->create_publisher<msg_interfaces::msg::MatrixProto>("/routing_request", 10);

        signal(SIGINT, mySigintHandler); // 放到NodeHandle 和 Rate 后面

        traffic_light_data.data = "non";
        remoteControlFlag.data = true;

        std::cout << "start server" << std::endl;

        struct sockaddr_in cliaddr;
        socklen_t cliaddr_len;
        int connfd;

        // socket   init
        listenfd = socket(AF_INET, SOCK_STREAM, 0);
        if (listenfd == -1)
        {
            std::cout << "Error  socket" << std::endl;
        }
        // bind   port
        int port = 12345;
        // 先声明才能get到参数
        this->declare_parameter<int>("server_port", SERV_PORT);
        this->get_parameter("server_port", port);
        std::cout << "port:" << port << std::endl;
        struct sockaddr_in addr;
        addr.sin_family = AF_INET;
        addr.sin_port = htons(port);
        addr.sin_addr.s_addr = INADDR_ANY;
        int opt = 1;
        setsockopt(listenfd, SOL_SOCKET, SO_REUSEPORT, &opt, sizeof(opt));
        if (bind(listenfd, (struct sockaddr *)&addr, sizeof(addr)) == -1)
        {
            std::cout << "Error bind" << std::endl;
        }

        // listener
        if (listen(listenfd, LISTEN_COUNT) == -1)
        {
            std::cout << "Error listen" << std::endl;
        }

        // add send thread
        // pthread_mutex_init(&cm_mutex, NULL);
        // pthread_detach(circular_trans_th);

        pthread_t pth;
        pthread_create(&pth, NULL, waitaccept, (void *)&listenfd);

        timer_ = this->create_wall_timer(
            100ms, std::bind(&PadServer::TimerCallback, this));
    }
    ~PadServer()
    {
    }

private:

    rclcpp::TimerBase::SharedPtr timer_;

    rclcpp::Publisher<msg_interfaces::msg::MatrixProto>::SharedPtr pad_pub;

    rclcpp::Subscription<msg_interfaces::msg::MatrixProto>::SharedPtr server_sub;

    void CallBackServer(msg_interfaces::msg::MatrixProto::SharedPtr msg)
    {
        std::shared_ptr<std::vector<uint8_t>> vec =
            std::make_shared<std::vector<uint8_t>>();
        vec->resize(msg->len);
        memcpy(vec->data(), msg->buf.data(), msg->len);
        // 反序列化
        chassis::BusEspInfo bus_esp_info_;
        bus_esp_info_.ParseFromArray(vec->data(), vec->size());
        if (bus_esp_info_.esp_veh_spd() * 3.6 < 1 && bus_esp_info_.esp_veh_spd() > 0)
            speed = 0;
        else
            speed = bus_esp_info_.esp_veh_spd() * 3.6;
    }
    static void mySigintHandler(int sig)
    {
        std::cout << "shutting down\n";
        rclcpp::shutdown();
        exit(0);
    }
    static void *waitaccept(void *arg)
    {
        // cliaddr_len = sizeof(cliaddr);
        while (1)
        {
            int fd = *(int *)arg;
            int connfd = accept(fd, NULL, NULL);
            {
                cm.connfd = connfd;
                // cm.cliaddr = cliaddr;
                // pthread_mutex_lock(&cm_mutex);
                all_connect.push_back(cm);
                // pthread_mutex_unlock(&cm_mutex);
            }

            char sendbuf[20] = {'\0'};
            // RCLCPP_INFO(this->get_logger(),
            //             "connected : %s:%d\r", inet_ntop(AF_INET, (void *)&cliaddr.sin_addr, sendbuf, 16), cliaddr.sin_port);
            std::cout << "~~~~~~~~~~~connect~~~~~~~~~~\n";
            pthread_t th;
            pthread_create(&th, NULL, msgRecv, (void *)&connfd);
            // pthread_detach(th);

            pthread_t circular_trans_th;
            pthread_create(&circular_trans_th, NULL, circular_trans, (void *)&connfd);
        }
    }
    static void *msgRecv(void *arg)
    {
        // iau_ros_msgs::VehicleHmi msg_control;
        int newsock = *(int *)arg;
        char buf[1024];
        int buff;

        while (true)
        {
            memset(buf, 0, sizeof(buf));
            int len = recv(newsock, buf, sizeof(buf), 0);
            if (len > 0)
            {
                std::cout << buf << std::endl;
                cJSON *root = cJSON_Parse(buf);
                std::string tag = cJSON_GetObjectItem(root, "tag")->valuestring;
                // yjkj_chassis_msgs::DrivingMode pad_drive;
                if (tag == "autoStart") // 获取车机控制模式
                {
                    std::cout << "start  send control" << std::endl;
                    cJSON *data = cJSON_GetObjectItem(root, "data");
                    pad_angle = (cJSON_GetObjectItem(data, "angle"))->valuedouble;
                    buff = (cJSON_GetObjectItem(data, "autoStart"))->valueint;
                    pad_lon = cJSON_GetObjectItem(data, "stationLongitude")->valuedouble;
                    pad_lat = cJSON_GetObjectItem(data, "stationLatitude")->valuedouble;

                    if (pad_lat != 0 || pad_lon != 0)
                    {
                        // pad直接发
                        ms = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch());
                    }
                }

                cJSON_free(root);
            }
            else
            { // 通过数据发送失败识别客户端断开连接，结束对应线程
                pthread_exit(NULL);
            }
        }
    }

    static void *circular_trans(void *msg)
    {
        int connfd = *(int *)msg;
        while (true)
        {
            // while (all_connect.empty()) {
            //     std::cout << "sleep3sec\n";
            //     sleep(3);
            // }
            sleep(1);
            // pthread_mutex_lock(&cm_mutex);
            // for (std::vector<connect_message>::iterator it = all_connect.begin(); it != all_connect.end(); ++it) {
            cJSON *root = cJSON_CreateObject();
            cJSON *data = cJSON_CreateObject();
            cJSON_AddItemToObject(data, "carDirection", cJSON_CreateNumber(lightType));
            cJSON_AddItemToObject(data, "carSpeed", cJSON_CreateNumber(speed));
            cJSON_AddItemToObject(data, "forObstacle", cJSON_CreateBool(false));
            cJSON_AddItemToObject(data, "leftObstacle", cJSON_CreateBool(false));
            cJSON_AddItemToObject(data, "rightObstacle", cJSON_CreateBool(false));
            cJSON_AddItemToObject(data, "carState", cJSON_CreateNumber(state));
            cJSON_AddItemToObject(data, "cloudEnable", cJSON_CreateBool(cloud_enable));
            cJSON_AddItemToObject(data, "remoteControlEnable", cJSON_CreateBool(remoteControlEnable));

            // debuginfo
            cJSON_AddItemToObject(data, "timestamp", cJSON_CreateString(timestamp.c_str()));
            cJSON_AddItemToObject(data, "source", cJSON_CreateString(source.c_str()));
            cJSON_AddItemToObject(data, "status", cJSON_CreateString(status.c_str()));

            if (roadLimit == 0)
            {
                roadLimit = 40;
            }
            cJSON_AddItemToObject(data, "roadLimit", cJSON_CreateNumber(roadLimit * 3.6));
            cJSON_AddItemToObject(root, "data", data);
            cJSON_AddItemToObject(root, "tag", cJSON_CreateString("carInfo"));
            std::string str = cJSON_Print(root);
            cJSON_Delete(root);
            int len = send(connfd, str.c_str(), str.size(), 0);
            if (len < 0)
            {
                // 数据发送失败的时候，连接已经断开，应该马上跳出for重新读取all_connect，否则下一次循环很可能还会识别到被删掉的数据，造成越界访问错误
                close(connfd);
                char sendbuf[20] = {'\0'};
                // RCLCPP_INFO(get_logger(), "Disconnected");
                std::cout << "Disconnected!!!\n";
                break;
            }
        }
    }
    void TimerCallback()
    {
        if (pad_lat != 0 || pad_lon != 0)
        {
            header::Header header;
            // routing::Poi p;
            std::shared_ptr<routing::RoutingRequest> routing_request = std::make_shared<routing::RoutingRequest>();
            header.set_stamp(ms.count());
            header.set_frame_id("routing");

            auto p =  routing_request->add_points();
            p->set_latitude_deg(pad_lat);
            p->set_longitude_deg(pad_lon);
            p->set_yaw(pad_angle);
            // p->set_latitude_deg(0);
            // p->set_longitude_deg(0);
            // p->set_yaw(0);
            routing_request->mutable_header()->CopyFrom(header);
            p->mutable_header()->CopyFrom(header);
            // routing_request->mutable_points()->CopyFrom(p);
            auto message = msg_interfaces::msg::MatrixProto();
            const size_t data_size = routing_request->ByteSizeLong();
            std::vector<uint8_t> data(data_size);
            routing_request->SerializeToArray(data.data(), data_size);
            message.len = data_size;
            message.buf = data;
            pad_pub->publish(message);
        }
    }
};
int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<PadServer>());
    rclcpp::shutdown();
    return 0;
}
